#ifndef PROGRAM_INTERACTION_H
#define PROGRAM_INTERACTION_H

#include <iostream>
#include <fstream>
#include <sstream>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <Eigen/Dense>
#include <vector>
#include <array>

//
// program type
//
typedef enum
{
    WAYPOINT_POINT_TYPE_JOINT,
    WAYPOINT_POINT_TYPE_CARTESIAN,
    WAYPOINT_POINT_TYPE_COUNT
} WayPointPointType;

typedef enum
{
    WAYPOINT_MOTION_TYPE_JOINT,
    WAYPOINT_MOTION_TYPE_LINE,
    WAYPOINT_MOTION_TYPE_COUNT
} WayPointMotionType;


inline std::istringstream & operator>>(std::istringstream & str, WayPointPointType & t){
    unsigned int tt = 0;
    if (!(str >> tt))
      return str;
    if (tt >= WAYPOINT_POINT_TYPE_COUNT){
      str.setstate(str.rdstate() | std::ios::failbit);
      return str;
    }
    t = static_cast<WayPointPointType>(tt);
    return str;
}

inline std::istringstream & operator>>(std::istringstream & str, WayPointMotionType & t){
    unsigned int tt = 0;
    if (!(str >> tt))
        return str;
    if (tt >= WAYPOINT_MOTION_TYPE_COUNT) {
        str.setstate(str.rdstate() | std::ios::failbit);
        return str;
    }
    t = static_cast<WayPointMotionType>(tt);
    return str;
}

struct wayPointType{
    WayPointPointType pointType;
    WayPointMotionType motionType;
    int index;
    Eigen::Matrix<double, 1, 6> point;
};

struct ProgramSpeed{
    double programJointSpeed;
    double programCartesianPosSpeed;
    double programCartesianRotationSpeed;
};



//
// Convertion function
//
WayPointPointType convertFromStringToWPPointType(const std::string &);

std::string convertFromWPPointTypeToString(WayPointPointType);

inline WayPointPointType convertFromIntToWPPointType(int pointTypeInt)
{
    if (pointTypeInt >= WAYPOINT_POINT_TYPE_COUNT)
        return WAYPOINT_POINT_TYPE_COUNT;
    else
        return static_cast<WayPointPointType>(pointTypeInt);
}

inline WayPointMotionType convertFromIntToWPMotionType(int motionTypeInt)
{
    if (motionTypeInt >= WAYPOINT_MOTION_TYPE_COUNT)
        return WAYPOINT_MOTION_TYPE_COUNT;
    else
        return static_cast<WayPointMotionType>(motionTypeInt);
}









#endif // PROGRAM_INTERACTION_H
